package es.uji.viselab.robot.links.fanuc;

import javax.ws.rs.core.MediaType;
import javax.ws.rs.core.MultivaluedMap;
import javax.ws.rs.core.Response;

import com.sun.jersey.api.client.Client;
import com.sun.jersey.api.client.ClientHandlerException;
import com.sun.jersey.api.client.ClientResponse;
import com.sun.jersey.api.client.UniformInterfaceException;
import com.sun.jersey.api.client.WebResource;
import com.sun.jersey.core.util.MultivaluedMapImpl;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RobotException;

public class RobotCommunicationFanucARCMate120 {

	Client client;
	protected String serverIP;
	protected int serverPort;
	protected String serverAddress;
	WebResource webResource;
	private String serverRESTresource;

	public RobotCommunicationFanucARCMate120() {
		this("150.128.86.214", 9999, 3000);
	}

	/**
	 * Creates the class Robot and initializates the client with the address of
	 * the server. As this method does not create yet a connection to the server
	 * it is not required to have an active server neither, this method does not
	 * return any exception
	 * 
	 * @param s
	 *            erverIP a String with the IP of the server
	 * 
	 * @param serverPort
	 *            an int with the port number where the server will be listening
	 * @param timeout
	 *            miliseconds to wait until a RobotException (related to
	 *            timeout) will be raised
	 */
	public RobotCommunicationFanucARCMate120(String serverIP, int serverPort, int timeout) {

		this.serverIP = serverIP;
		this.serverPort = serverPort;
		this.serverRESTresource = "/fanuc/RestServiceImpl.svc";
		this.serverAddress = "http://" + serverIP + ":" + serverPort + serverRESTresource;

		client = Client.create();
		client.setConnectTimeout(timeout);

		webResource = client.resource(serverAddress);
	}

	/**
	 * Does a ping to the robot server. This is useful to check that the server
	 * is listening and accepts connections, but this ping do not have anything
	 * to do with the Robot, so it is not needed to have it connected
	 * 
	 * This method returns nothing if everything went well, but a RobotException
	 * is something went wrong
	 * 
	 * @return
	 */
	public String Ping() throws RobotException {

		RobotResponse rr = new RobotResponse();
		webResource = client.resource(serverAddress + "/ping");

		String s;
		try {
			s = webResource.get(String.class);
			rr.setXml(s);
			return rr.getMessage();

		} catch (ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		} catch (RobotException e) {
			throw new RobotException(e.getMessage());
		}

	}

	/**
	 * Orders the server to connect to the robot.
	 * 
	 * @throws RobotException
	 *             in case the request was not accepted or the message returned
	 *             is an error
	 */
	public void Connect() throws RobotException {
		WebResource service = null;
		ClientResponse response = null;
		MultivaluedMap<String, String> queryParams = new MultivaluedMapImpl();
		// queryParams.add("ip", robotIP);
		try {
			service = client.resource(serverAddress);
			response = service.path("fanuc").path("connection")
					.type(MediaType.APPLICATION_FORM_URLENCODED).accept(MediaType.APPLICATION_XML)
					.put(ClientResponse.class, queryParams);
			if (Response.Status.CREATED.getStatusCode() != response.getStatus()) {
				throw new RobotException("Unexpected response from the server, status: "
						+ response.getStatus() + ". " + response.getEntity(String.class));
			}
		} catch (com.sun.jersey.api.client.ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}
		/*
		 * String body; String s; RobotResponse rr = new RobotResponse(); body =
		 * "<xml><operation>Connect</operation></xml>"; try { s =
		 * webResource.type("text/xml").header("body", body).put(String.class);
		 * rr.setXml(s); if (!rr.getMessage().equals("Connected")) { throw new
		 * RobotException("Connect() unexpected returned message: " +
		 * rr.getMessage() + " ('Connected' was expected)"); } } catch
		 * (ClientHandlerException e) { throw new
		 * RobotException(e.getMessage()); } catch (UniformInterfaceException e)
		 * { throw new RobotException(e.getMessage(),
		 * e.getResponse().getStatus()); }
		 */
	}

	/**
	 * Orders the robot to move its joints to certain degrees
	 * 
	 * @param j
	 *            an object of type Joints that contains the 6 joints in degrees
	 * @throws RobotException
	 */
	public void Move(Joints j) throws RobotException {
		WebResource service = null;
		ClientResponse response = null;
		MultivaluedMap<String, String> postParams = new MultivaluedMapImpl();
		postParams.add("joint1", String.valueOf((int) (j.getJoint1() * 360 / (2*Math.PI))));
		postParams.add("joint2", String.valueOf((int) (j.getJoint2() * 360 / (2*Math.PI))));
		postParams.add("joint3", String.valueOf((int) (j.getJoint3() * 360 / (2*Math.PI))));
		postParams.add("joint4", String.valueOf((int) (j.getJoint4() * 360 / (2*Math.PI))));
		postParams.add("joint5", String.valueOf((int) (j.getJoint5() * 360 / (2*Math.PI))));
		postParams.add("joint6", String.valueOf((int) (j.getJoint6() * 360 / (2*Math.PI))));
		MultivaluedMap<String, String> queryParams = new MultivaluedMapImpl();
		queryParams.add("format", "joint");

		try {
			service = client.resource(serverAddress);
			response = service.queryParams(queryParams).path("fanuc").path("position")
					.post(ClientResponse.class, postParams);
			if (Response.Status.OK.getStatusCode() != response.getStatus()) {
				throw new RobotException("Unexpected response from the server, status: "
						+ response.getStatus() + ". " + response.getEntity(String.class));
			}
		} catch (com.sun.jersey.api.client.ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}

		
	}

	public void Move(RobotToolLocation rtl) throws RobotException {
		WebResource service = null;
		ClientResponse response = null;
		MultivaluedMap<String, String> postParams = new MultivaluedMapImpl();
		postParams.add("x", String.valueOf((int)rtl.getLocation().getX()) );
		postParams.add("y", String.valueOf((int)rtl.getLocation().getY()) );
		postParams.add("z", String.valueOf((int)rtl.getLocation().getZ()) );
		postParams.add("w", "-180");//String.valueOf((int)rtl.getOrientation().getX()));
		postParams.add("p", String.valueOf((int)rtl.getOrientation().getY()));
		postParams.add("r", String.valueOf((int)rtl.getOrientation().getZ()));
		MultivaluedMap<String, String> queryParams = new MultivaluedMapImpl();
		queryParams.add("format", "XYZ");
		
		try {
			service = client.resource(serverAddress);
			response = service.queryParams(queryParams).path("fanuc").path("position")
					.post(ClientResponse.class, postParams);
		    if (Response.Status.OK.getStatusCode() != response.getStatus()) {
				throw new RobotException("Unexpected response from the server, status: "
						+ response.getStatus() + ". " + response.getEntity(String.class));
			}
		} catch (com.sun.jersey.api.client.ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}
	}

	public RobotToolLocation getXYZWPR() throws RobotException {
		MultivaluedMap<String, String> queryParams = new MultivaluedMapImpl();
		queryParams.add("format", "XYZ");
		String responseXML;

		try {
			WebResource service = client.resource(serverAddress);
			ClientResponse response = service.queryParams(queryParams).path("fanuc")
					.path("position").get(ClientResponse.class);
			if (Response.Status.OK.getStatusCode() == response.getStatus()) {

				// See changelog 01/03/2012
				// RobotPosition pos = response.getEntity(RobotPosition.class);
				// I should do it manually
				responseXML = response.getEntity(String.class);
				RobotPosition pos = new RobotPosition(responseXML);
				// pos.setX(pos.getX()/1000);
				// pos.setY(pos.getY()/1000);
				// pos.setZ(pos.getZ()/1000);
				return new RobotToolLocation(pos);
			} else {
				throw new RobotException("Unexpected response from the server, status: "
						+ response.getStatus() + ". " + response.getEntity(String.class));
			}
		} catch (com.sun.jersey.api.client.ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}

		/*
		 * RobotResponse rr = new RobotResponse(); webResource =
		 * client.resource(serverAddress + "/CurrentPositionXYZWPR"); String s;
		 * 
		 * try { s = webResource.get(String.class); rr.setXml(s);
		 * RobotToolLocation rtl = rr.toToolLocation(); return rtl; } catch
		 * (ClientHandlerException e) { throw new
		 * RobotException(e.getMessage()); } catch (UniformInterfaceException e)
		 * { throw new RobotException(e.getMessage(),
		 * e.getResponse().getStatus()); } catch (RobotException e) { throw new
		 * RobotException(e.getMessage()); }
		 */

	}

	public Joints getJoints() throws RobotException {

		MultivaluedMap<String, String> queryParams = new MultivaluedMapImpl();
		String responseXML;
		queryParams.add("format", "joint");

		try {
			WebResource service = client.resource(serverAddress);
			ClientResponse response = service.queryParams(queryParams).path("fanuc")
					.path("position").get(ClientResponse.class);
			if (Response.Status.OK.getStatusCode() == response.getStatus()) {
				// See changelog 01/03/2012
				//RobotPosition pos = response.getEntity(RobotPosition.class);
				  // I should do it manually
				responseXML =  response.getEntity(String.class);
				RobotPosition pos = new RobotPosition(responseXML);
				pos.setJoint1(pos.getJoint1()*2*Math.PI/360);
				pos.setJoint2(pos.getJoint2()*2*Math.PI/360);
				pos.setJoint3(pos.getJoint3()*2*Math.PI/360);
				pos.setJoint4(pos.getJoint4()*2*Math.PI/360);
				pos.setJoint5(pos.getJoint5()*2*Math.PI/360);
				pos.setJoint6(pos.getJoint6()*2*Math.PI/360);
				return new Joints(pos);
			} else {
				throw new RobotException("Unexpected response from the server, status: "
						+ response.getStatus() + ". " + response.getEntity(String.class));
			}
		} catch (com.sun.jersey.api.client.ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}


	}

	public boolean isConnected() throws RobotException {

		RobotResponse rr = new RobotResponse();
		webResource = client.resource(serverAddress + "/IsConnected");
		String s;

		try {
			s = webResource.get(String.class);
			if (s.length() > 0) {
				rr.setXml(s);
				String message;
				message = rr.getMessage();
				int value;
				value = Integer.parseInt(message);
				if (value == 1) {
					return true;
				} else {
					return false;
				}
			} else {
				throw new RobotException("I am not capable to get an answer from the server");
			}
		} catch (ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		} catch (RobotException e) {
			throw new RobotException(e.getMessage());
		}

	}

	/**
	 * Orders the server to disconnect to the robot.
	 * 
	 * @throws RobotException
	 *             in case the request was not accepted or the message returned
	 *             is an error
	 */
	public void Disconnect() throws RobotException {

		String body;
		String s;
		RobotResponse rr = new RobotResponse();
		body = "<xml><operation>Disconnect</operation></xml>";
		try {
			s = webResource.type("text/xml").header("body", body).post(String.class);
			rr.setXml(s);
			if (!rr.getMessage().equals("disconnected")) {
				throw new RobotException("Disconnect() unexpected returned message: "
						+ rr.getMessage() + " ('Disconnected' was expected)");
			}
		} catch (ClientHandlerException e) {
			throw new RobotException(e.getMessage());
		} catch (UniformInterfaceException e) {
			throw new RobotException(e.getMessage(), e.getResponse().getStatus());
		}

	}

}
